#ifndef __DALELIB_MATRIX_h
#define __DALELIB_MATRIX_h

#include <iostream>
#include <vector>

using namespace std;

namespace dalelib
{

template <class T>
class matrix
{
private:

	int _capacity;
	int _size;
	vector<T> _data;

	void allocate()
	{
		if( _capacity<_size ) _capacity = _size;
		_data.resize(_capacity*_capacity);
	}

public:

	matrix(const int _Size)
		: _size(_Size)
	{
		_capacity = _size;
		allocate();
	}

	matrix(const int _Size,const int _Capacity)
		: _size(_Size)
		, _capacity(_Capacity)
	{
		allocate();
	}

	void setSize(const int _NewSize)
	{
		_size = _NewSize;
		allocate();
	}

	void setCapacity(const int _NewCapacity)
	{
		_capacity = _NewCapacity;
		allocate();
	}

	int getSize() { return _size; };
	int getCapacity() { return _capacity; };

	void getValue(const int _Row,const int _Column,T& _Value, bool& _Success)
	{
		if( (_Row>=_size) || (_Column>=_size) || (_Row<0) || (_Column<0) )
		{
			_Success = false;
			return;
		}
		_Value = _data[_Row*_capacity+_Column];
		_Success = true;
	}

	bool setValue(const int _Row,const int _Column,const T _Value)
	{
		if( (_Row >= _size) || (_Column >= _size) || (_Row<0) || (_Column<0) ) return false;
		_data[_Row*_capacity+_Column] = _Value;
		return true;
	}

	void compareToIdentity()
	{
		int worstdiagonal = 0;
		T maxunitydeviation = 0.0;
		T currentunitydeviation;
		for( int i=0; i<_size; ++i )
		{
			currentunitydeviation = _data[i*_capacity+i] - 1.;
			if( currentunitydeviation<0.0) currentunitydeviation *= -1.;
			if( currentunitydeviation>maxunitydeviation )
			{
				maxunitydeviation = currentunitydeviation;
				worstdiagonal = i;
			}
		}
		int worstoffdiagonalrow = 0;
		int worstoffdiagonalcolumn = 0;
		T maxzerodeviation = 0.0;
		T currentzerodeviation ;
		for( int i=0; i<_size; ++i )
		{
			for( int j=0; j<_size; ++j )
			{
				if( i == j ) continue;  // we look only at non-diagonal terms
				currentzerodeviation = _data[i*_capacity+j];
				if( currentzerodeviation<0.0) currentzerodeviation *= -1.0;
				if( currentzerodeviation>maxzerodeviation )
				{
					maxzerodeviation = currentzerodeviation;
					worstoffdiagonalrow = i;
					worstoffdiagonalcolumn = j;
				}
			}
		}
		cout << "Worst diagonal value deviation from unity: " << maxunitydeviation << " at _Row/_Column " << worstdiagonal << endl;
		cout << "Worst off-diagonal value deviation from zero: " << maxzerodeviation << " at _Row = " << worstoffdiagonalrow << ", _Column = " << worstoffdiagonalcolumn << endl;
	}

	void setToProduct(matrix& _Left, matrix& _Right)
	{
		_size = _Left.getSize();
		allocate();

		for( int i=0; i<_size; ++i )
		{
			for( int j=0; j<_size; ++j )
			{
				T sum = 0.0;
				T leftvalue, rightvalue;
				bool success;
				for( int c=0; c<_size; ++c )
				{
					_Left.getValue(i,c,leftvalue,success);
					_Right.getValue(c,j,rightvalue,success);
					sum += leftvalue * rightvalue;
				}
				setValue(i,j,sum);
			}
		}
	}

	void copymatrix(matrix& _Source)
	{
		_size = _Source.getSize();
		allocate();

		for( int i=0; i<_size; ++i )
		{
			for( int j=0; j<_size; ++j )
			{
				T value;
				bool success;
				_Source.getValue(i,j,value,success);
				_data[i*_capacity+j] = value;
			}
		}
	}

	void invert()
	{
		if(_size <= 0) return;  // sanity check
		if(_size == 1) return;  // must be of dimension >= 2

		for( int i=1; i<_size; ++i ) _data[i] /= _data[0]; // normalize row 0

		for( int i=1; i<_size; ++i )
		{
			for( int j=i; j<_size; ++j )
			{ // do a column of L
				T sum = 0.0;
				for( int k=0; k<i; ++k )
				sum += _data[j*_capacity+k] * _data[k*_capacity+i];
				_data[j*_capacity+i] -= sum;
			}
			if( i==_size-1 ) continue;
			for( int j=i+1; j<_size; ++j )
			{  // do a row of U
				T sum = 0.0;
				for( int k=0; k<i; ++k ) sum += _data[i*_capacity+k]*_data[k*_capacity+j];
				_data[i*_capacity+j] = (_data[i*_capacity+j]-sum) / _data[i*_capacity+i];
			}
		}

		for( int i=0; i<_size; ++i )  // invert L
		{
			for( int j=i; j<_size; ++j )
			{
				T x = 1.0;
				if( i!=j )
				{
					x = 0.0;
					for( int k=i; k<j; ++k )
					x -= _data[j*_capacity+k]*_data[k*_capacity+i];
				}
				_data[j*_capacity+i] = x/_data[j*_capacity+j];
			}
		}
		for( int i=0; i<_size; ++i )   // invert U
		{
			for( int j=i; j<_size; ++j )
			{
				if( i==j ) continue;
				T sum = 0.0;
				for( int k=i; k<j; ++k ) sum += _data[k*_capacity+j]*( (i==k) ? 1.0 : _data[i*_capacity+k] );
				_data[i*_capacity+j] = -sum;
			}
		}

		for( int i=0; i<_size; ++i )   // final inversion
		{
			for( int j=0; j<_size; ++j )
			{
				T sum = 0.0;
				for( int k=((i>j)?i:j); k<_size; ++k ) sum += ((j==k)?1.0:_data[j*_capacity+k])*_data[k*_capacity+i];
				_data[j*_capacity+i] = sum;
			}
		}
	}
	
	bool transformVector(vector<T>& _Vector)
	{
		if( _Vector.size()!=_size ) return false;
		T* temp = new T[_size];
		
		for( int i=0; i<_size; ++i )
		{
			int index = i*_capacity;
			temp[i] = 0.f;
			for( int j=0; j<_size; ++j )
			{
				temp[i] += _Vector[i]*_data[index++];
			}
		}
		memcpy(&_Vector[0],temp,_size*sizeof(T));
		delete temp;
		return true;
	}
	
	void dump()
	{
		for( int i=0; i<_size; ++i )
		{
			cout << "i=" << i << ": ";
			for( int j=0; j<_size; ++j )
			{
				cout << _data[i+j*_capacity] << " ";
			}
			cout << endl;
		}
	}
};

} // namespace dalelib

#endif
